#pragma once
#ifndef _CONFIG_H_
#define _CONFIG_H_

#include"common_include.h"

namespace vld_imu_slam{
class Config{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<Config> Ptr;

    string pointcloud_topic_,imu_topic_,pointcloud_format_;

    int n_scan_,num_points_a_line_,plane_cluster_points_num_,nonGround_sample_points_num_,
        ground_sample_points_num_,numberOfCores_;

    double plane_roughness_threshold_,conner_roughness_threshold_,imu_gravity_,
                    imu_acc_noise_std_,imu_gyr_noise_std_,
                    imu_acc_bias_n_,imu_gyr_bias_n_,
                    kf_angle_threshold_,kf_trans_threshold_;

    Eigen::Matrix3d ext_imu_lidar_rot_;
    Eigen::Vector3d ext_imu_lidar_trans_;


    Config(){};
    Config(ros::NodeHandle& nh )
    {
        nh.param<std::string>("vld_imu_slam/pointcloud_topic", pointcloud_topic_, "points_raw");
        nh.param<std::string>("vld_imu_slam/pointcloud_format", pointcloud_format_, "CustomMsg");
        nh.param<std::string>("vld_imu_slam/imu_topic", imu_topic_, "imu_correct");
        nh.param<int>("vld_imu_slam/n_scan", n_scan_, 6);
        nh.param<int>("vld_imu_slam/num_points_a_line", num_points_a_line_, 4000);
        nh.param<double>("vld_imu_slam/plane_roughness_threshold", plane_roughness_threshold_, 5);
        nh.param<double>("vld_imu_slam/conner_roughness_threshold", conner_roughness_threshold_, 40);
        nh.param<int>("vld_imu_slam/plane_cluster_points_num", plane_cluster_points_num_, 8);
        nh.param<int>("vld_imu_slam/nonGround_sample_points_num", nonGround_sample_points_num_, 180);
        nh.param<int>("vld_imu_slam/ground_sample_points_num", ground_sample_points_num_, 180);
        nh.param<int>("vld_imu_slam/numberOfCores", numberOfCores_, 4);
        nh.param<double>("vld_imu_slam/imu_gravity", imu_gravity_, 5);
        nh.param<double>("vld_imu_slam/imu_acc_noise_std", imu_acc_noise_std_, 40);
        nh.param<double>("vld_imu_slam/imu_gyr_noise_std", imu_gyr_noise_std_, 40);
        nh.param<double>("vld_imu_slam/imu_acc_bias_n", imu_acc_bias_n_, 40);
        nh.param<double>("vld_imu_slam/imu_gyr_bias_n", imu_gyr_bias_n_, 40);
        nh.param<double>("vld_imu_slam/kf_angle_threshold", kf_angle_threshold_, 40);
        nh.param<double>("vld_imu_slam/kf_trans_threshold", kf_trans_threshold_, 40);

        vector<double> ext_imu_lidar_rotV;
        vector<double> ext_imu_lidar_transV;
        nh.param<vector<double>>("vld_imu_slam/extrinsic_imu_lidar_rot", ext_imu_lidar_rotV, vector<double>());
        nh.param<vector<double>>("vld_imu_slam/extrinsic_imu_lidar_trans", ext_imu_lidar_transV, vector<double>());
        ext_imu_lidar_rot_ = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(ext_imu_lidar_rotV.data(), 3, 3);
        ext_imu_lidar_trans_ = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(ext_imu_lidar_transV.data(), 3, 1);



    }

};



}
#endif
